`timescale 1ns / 1ns

module arm_top(
    input   wire            sys_clk ,
    input   wire            reset_n ,
    input   wire            en_flag , // 开始抓取

    input   wire    [9:0]   x_in    , // 抓取像素位置x
    input   wire    [9:0]   y_in    , // 抓取像素位置y
    input   wire    [2:0]   grade   ,
    input   wire    [1:0]   x_out   , // 放置位置x 0,1,2,3
    input   wire    [1:0]   y_out   , // 放置位置y 0,1,2

    input   wire            uart_rx , // 接收回读数据
    output  wire            uart_tx , // 发送回读数据

    output  wire            camera_updata , // 当机械臂移动到目的地时给摄像头的信号
    output  wire            arm_finish // 抓取结束
);

wire    [7:0]   x_o    ;
wire    [7:0]   y_o    ;

// 输出会出现0
change_xyz change_xyz_inst
(
    .x_in    (x_in ), // 抓取像素位置x
    .y_in    (y_in ), // 抓取像素位置y

    .x_o     (x_o  ), // 抓取位置x
    .y_o     (y_o  )  // 抓取位置y
);

wire  [15:0]    dir_dis_out      ;
wire  [ 3:0]    rom_d_add        ;
assign rom_d_add =  {y_out,x_out}; // 拼接

// 取出存放位置（有一个周期的滞后）
rom_d rom_d_inst 
(
    .address    ( rom_d_add     ),
    .clock      ( sys_clk       ),
    .q          ( dir_dis_out   )
);

// 机械臂控制参数
wire    [11:0]  claws;
wire    [7:0]   out_x;
wire    [7:0]   out_y;
wire            out_z;
wire    [11:0]  out_time;

wire            centure_move_flag; // 确定好机械臂的移动位置
wire            dir              ; // 机械臂移动方向（目的地or仓库）
wire            begin_reg        ;
wire            real_en          ;

// 机械臂运动的姿态
centure_move centure_move_inst
(
    .sys_clk    (sys_clk    ),
    .reset_n    (reset_n    ),
    .en_flag    (en_flag    ), // 开始规划机械臂路径（总）
    .begin_reg  (begin_reg  ), // 进行下一步移动（分）

    .in_x       (x_o        ), // 抓取的x
    .in_y       (y_o        ), // 抓取的y
    .to_x       (dir_dis_out[15:8]), // 松开的x
    .to_y       (dir_dis_out[ 7:0]), // 松开的y
    .move_time  (12'd450    ), // 移动时间

    // 输出给锡盟算法的数值
    .dir        (dir        ), // 抓取方向 1:目的地 0:仓库
    .claws      (claws      ),
    .out_x      (out_x      ),
    .out_y      (out_y      ),
    .out_z      (out_z      ),
    .real_en    (real_en    ),
    .out_time   (out_time   ),

    .camera_updata(camera_updata),
    .begin_flag (centure_move_flag), // 分步启动信号
    .finish_flag(arm_finish )
);

// 计算得到的角度信息
wire     [23:0]  seta_1_out;
wire     [23:0]  seta_2_out;
wire     [23:0]  seta_3_out;
wire             L_finish_flag;

// 机械臂逆运算
L_0_get L_0_get_inst
(
    .sys_clk        (sys_clk     ),
    .reset_n        (reset_n     ),
    .begin_flag     (centure_move_flag), // 开始算法计算
    // 输入坐标
    .x_in           (out_x       ),
    .y_in           (out_y       ),
    .z_in           (out_z       ),
    .d_in           (dir         ),
    // 逆运算输出角度
    .seta_1_out     (seta_1_out  ),
    .seta_2_out     (seta_2_out  ),
    .seta_3_out     (seta_3_out  ),
    // 输出信号
    .finish_flag    (L_finish_flag) // 计算完成信号
);

// 将角度值转化为机械臂能够识别的12位信号
wire    [11:0]  angle_1    ;
wire    [11:0]  angle_2    ;
wire    [11:0]  angle_3    ;
wire    [11:0]  angle_4    ;
wire            value_flag    ;

change_value change_value_inst
(
    .sys_clk    (sys_clk     ),
    .reset_n    (reset_n     ),
    .begin_flag (L_finish_flag), // 开始输出转换
    // 角度输入
    .grade      (grade       ),
    .seta_1_in  (seta_1_out  ),
    .seta_2_in  (seta_2_out  ),
    .seta_3_in  (seta_3_out  ),
    .dir        ( out_x[7]   ), 
    .tan_sign   ( dir        ), // 抓取方向 1:目的地 0:仓库

    // 数据输出
    .angle_1    (angle_1     ),
    .angle_2    (angle_2     ),
    .angle_3    (angle_3     ),
    .angle_4    (angle_4     ),

    .finish_flag(value_flag  )  // 转换完成
);

// PID校准值
wire    [11:0]  angle_1_real    ;
wire    [11:0]  angle_2_real    ;
wire    [11:0]  angle_3_real    ;
wire    [11:0]  angle_4_real    ;
wire    [11:0]  angle_5_real    ;
wire    [11:0]  angle_6_real    ;

wire            read_en         ;
wire            read_send       ;
wire            real_flag       ;

wire    [11:0]  pid_out_time    ;
wire            dispose_flag    ;
wire            reread_flag     ;

pid_rx pid_rx_inst
(
    .sys_clk        (sys_clk    ),
    .reset_n        (reset_n    ),
    .en_flag        (value_flag ), // 开始校准信号，也是控制机械臂信号
    .real_en        (real_en    ), // 是否需要校准
    .down_en        (out_z      ), // 特殊处理，下抓的时候保持上身不变，只动下半身
    .uart_rx        (uart_rx    ),

    .in_time        (out_time   ), // 移动时间
    .angle_1        (claws      ), // 目标值
    .angle_2        (dir ? angle_4 : angle_1 ), // 目标值
    .angle_3        (dir ? 12'd665 : 12'd2320), // 目标值 // 2335
    .angle_4        (angle_3    ), // 目标值
    .angle_5        (angle_2    ), // 目标值
    .angle_6        (angle_1    ), // 目标值
    // 与发送模块相衔接的信号
    .real_start     (read_en    ), // 开始校准中
    .real_send      (read_send  ), // 发送校准信号
    .real_flag      (real_flag  ), // 回读信息读取完毕

    // 实际控制机械臂的数据
    .angle_1_real   (angle_1_real), // 校正值
    .angle_2_real   (angle_2_real), // 校正值
    .angle_3_real   (angle_3_real), // 校正值
    .angle_4_real   (angle_4_real), // 校正值
    .angle_5_real   (angle_5_real), // 校正值
    .angle_6_real   (angle_6_real), // 校正值
    .out_time       (pid_out_time), // 输出时间

    .real_finish    (begin_reg   )  // 校准完毕(进行下一步)
);

uart_tx_test uart_tx_test_inst
(
    .clk         (sys_clk     ),
    .reset_n     (reset_n     ),
    // 驱动信号
    .en_flag     (read_send   ), // 开始控制/回读信息
    .rx_flag     (real_flag   ), // 回读信息完毕信号
    .read_en     (read_en     ), // 控制发送还是回读的钥匙

    // 输入控制信号
    .move_time   (pid_out_time),
    .angle_1_in  (angle_1_real),
    .angle_2_in  (angle_2_real),
    .angle_3_in  (angle_3_real),
    .angle_4_in  (angle_4_real),
    .angle_5_in  (angle_5_real),
    .angle_6_in  (angle_6_real),

    .uart_tx     (uart_tx     ),
    .trans_finish()
);

/************************************************************/
/*
vio4	vio4_inst_x
(
	.probe  ( angle_3    ),
	.source ( x_in       )
);

vio4	vio4_inst_y
(
	.probe  ( angle_2    ),
	.source ( y_in       )
);

vio4	vio4_inst_z
(
	.probe  ( angle_1    ),
	.source ()
);
*/

endmodule